#include "include.h"

static void ParamInit(
       PID_t *pid,
       float p,
       float i,
       float d,
       float integral_limit,
       float max_output)
{
    pid->p = p; 
    pid->i = i; 
    pid->d = d; 
    pid->integral_limit = integral_limit;
    pid->max_output = max_output; 
}

static void Reset(
        PID_t *pid,
        float p,
        float i,
        float d,
        float integral_limit,
        float max_output)
{ 
    pid->p = p; 
    pid->i = i; 
    pid->d = d; 
    pid->integral_limit = integral_limit;
    pid->max_output = max_output; 
    pid->last_err = 0;
    pid->target_value = 0;
    pid->current_value = 0;
    pid->i_value = 0;
}

static void Calculate(
        PID_t *pid,
        float current_value,float target_value)
{
    pid->current_value = current_value;
    pid->target_value = target_value;
    pid->err = pid->target_value - pid->current_value;
    float p_value = pid->p * pid->err; 
    float d_value = 0;
    if(pid->first_time_flag == 1)pid->first_time_flag = 0;  // 第一次不计算D
    else d_value = pid->d * (pid->err - pid->last_err);
    pid->i_value += pid->i * pid->err;
    if (pid->i_value > pid->integral_limit)
    {
        pid->i_value = pid->integral_limit;
    }
    if (pid->i_value < -pid->integral_limit)
    {
        pid->i_value = -pid->integral_limit;
    }
    pid->output = p_value + d_value + pid->i_value;
    pid->last_err = pid->err;
    if (pid->output > pid->max_output)
    {
        pid->output = pid->max_output;
    }
    if (pid->output < -pid->max_output)
    {
        pid->output = -pid->max_output;
    }
}

void PID_StructInit(PID_t *pid)
{
    pid->f_ParamInit = ParamInit;
    pid->f_Reset = Reset;
    pid->f_Calculate = Calculate;
    pid->last_err = 0;
    pid->target_value = 0;
    pid->current_value = 0;
    pid->i_value = 0;
    pid->first_time_flag = 1;
}

